#include "Swarm.h"
#include <QtCharts/QLineSeries>
#include<qdebug.h>

Swarm::Swarm()
{
    MAVLinkProtocol* mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol();
    connect(mavlinkProtocol, &MAVLinkProtocol::messageReceived, this, &Swarm::_receiveMessage);
    connect(&timer_status, &QTimer::timeout, this, &Swarm::update_status);
    timer_status.setInterval(500);
    timer_status.start();
}

Swarm::~Swarm()
{
}
void Swarm::update_status()
{
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    _vehiclelist=manager->vehicles();
    __uav_count=0;

    if(_vehiclelist && _vehiclelist->objectList()->count()>0)
    {
        int ready=0;
        for(int i=0;i<_vehiclelist->objectList()->count();i++)
        {
            QObject* obj =  _vehiclelist->get(i);
            Vehicle* vehicle0=static_cast<Vehicle*> (obj);
            //        if(_vehiclelist && _vehiclelist->objectList()->count()==1)
            //        {
            //            vehicle0->parameterManager()->refreshAllParameters();
            //        }

            if(vehicle0->healthAndArmingCheckReport()->canArm())
            {
                ready++;
            }

//                qDebug()<<"ready"<<vehicle0->healthAndArmingCheckReport()->canArm();
            if (!vehicle0->vehicleLinkManager()->communicationLost()) {
                __uav_count++;
        }
    }
        _ready_num=QString::number(ready);
        emit valueChanged2();
    _uavnum=QString::number(__uav_count);
    _uavcount=QString::number(_vehiclelist->objectList()->count());
    emit valueChanged();
    emit valueChanged3();
}
}
void Swarm::setUavnum(QString uavnum)
{
_uavnum=uavnum;
qDebug()<<uavnum;
emit valueChanged();
}

void Swarm::setReady_num(QString ready_num)
{
_ready_num=ready_num;
qDebug()<<ready_num;
emit valueChanged2();
}
void Swarm::setUavcount(QString uavcount)
{
_uavcount=uavcount;
qDebug()<<uavcount;
emit valueChanged3();
}
void Swarm::setTransdelay(QString transdelay)
{
_transdelay=transdelay;
qDebug()<<transdelay;
emit valueChanged4();
}
void Swarm::sendStartCommand()
{
    start_swarm=true;
//    if (!_vehicle)
//    {
//        qWarning() << "Internal error";
//        return;
//    }
//    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
//    _vehiclelist=manager->vehicles();

//    if(_vehiclelist && _vehiclelist->objectList()->count()>0)
//    {

//        for(int i=0;i<_vehiclelist->objectList()->count();i++)
//        {
//            QObject* obj =  _vehiclelist->get(i);
//            Vehicle* vehicle0=static_cast<Vehicle*> (obj);
//            if (vehicle0->id()==1)
//            {
//                WeakLinkInterfacePtr weakLink = vehicle0->vehicleLinkManager()->primaryLink();
//                if (!weakLink.expired()) {
//                    SharedLinkInterfacePtr sharedLink = weakLink.lock();

//                    if (!sharedLink) {
//                        qCDebug(VehicleLog) << "_handlePing: primary link gone!";
//                        return;
//                    }
//                    //            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
//                    auto priority_link =sharedLink;
//                    mavlink_message_t msg;

//                    mavlink_msg_gps_raw_int_pack_chan( 55,
//                                                       55,
//                                                      priority_link->mavlinkChannel(),
//                                                      &msg,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0,
//                                                      0);
//                        vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
//                    }
//                }
//            }
//        }



    }



void
Swarm::_receiveMessage(LinkInterface*, mavlink_message_t message)
{
if(message.msgid==MAVLINK_MSG_ID_GPS_RAW_INT)
{

    //qDebug()<<"receive MAVLINK_MSG_ID_TEST_MAVLINK";
    mavlink_gps_raw_int_t gps_raw;
    mavlink_msg_gps_raw_int_decode(&message,&gps_raw);
    qDebug()<<gps_raw.hdg_acc;
    if(gps_raw.hdg_acc==55)
    {
        start_swarm=false;
    }
   // qDebug()<<"start_swarm"<<start_swarm;
    if(message.sysid==1)
    {
    if (!_vehicle)
    {
                qWarning() << "Internal error";
                return;
    }
    auto *manager = qgcApp()->toolbox()->multiVehicleManager();
    _vehiclelist=manager->vehicles();

    if(_vehiclelist && _vehiclelist->objectList()->count()>0)
    {

                for(int i=0;i<_vehiclelist->objectList()->count();i++)
                {
                    QObject* obj =  _vehiclelist->get(i);
                    Vehicle* vehicle0=static_cast<Vehicle*> (obj);
                        WeakLinkInterfacePtr weakLink = vehicle0->vehicleLinkManager()->primaryLink();
                        if (!weakLink.expired()) {
                        SharedLinkInterfacePtr sharedLink = weakLink.lock();

                        if (!sharedLink) {
                            qCDebug(VehicleLog) << "_handlePing: primary link gone!";
                            return;
                        }
                        //            auto protocol = qgcApp()->toolbox()->mavlinkProtocol();
                        auto priority_link =sharedLink;
                        mavlink_message_t msg;
//                        uint16_t yaw=1;
//                        if(start_swarm)
//                        {
//                            yaw=55;
//                        }
                        if(vehicle0->id()==1)
                        {
                            currentDateTime = QDateTime::currentDateTime();
                            milliseconds = currentDateTime.currentMSecsSinceEpoch();
                            time_delay=milliseconds-premilliseconds;
                            premilliseconds=milliseconds;
                            _transdelay=QString::number(time_delay);
                            emit valueChanged4();
                        if(start_swarm){
                            mavlink_msg_gps_raw_int_pack_chan(55,
                                                              55,
                                                              priority_link->mavlinkChannel(),
                                                              &msg,
                                                              gps_raw.time_usec,
                                                              gps_raw.fix_type,
                                                              gps_raw.lat,
                                                              gps_raw.lon,
                                                              gps_raw.alt,
                                                              gps_raw.eph,
                                                              gps_raw.epv,
                                                              gps_raw.vel,
                                                              0,
                                                              0,
                                                              0,
                                                              0,
                                                              0,
                                                              0,
                                                              gps_raw.hdg_acc,
                                                              gps_raw.yaw);
                            vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);
                        }
                        else{
                            mavlink_msg_gps_raw_int_pack_chan( 1,
                                                              1,
                                                              priority_link->mavlinkChannel(),
                                                              &msg,
                                                              gps_raw.time_usec,
                                                              gps_raw.fix_type,
                                                              gps_raw.lat,
                                                              gps_raw.lon,
                                                              gps_raw.alt,
                                                              gps_raw.eph,
                                                              gps_raw.epv,
                                                              gps_raw.vel,
                                                              0,
                                                              0,
                                                              0,
                                                              0,
                                                              0,
                                                              0,
                                                              gps_raw.hdg_acc,
                                                              gps_raw.yaw);
                            vehicle0->sendMessageOnLinkThreadSafe(sharedLink.get(), msg);

                        }
                        }

                        }
                    }

    }
    }
}
}
